#ifndef __GIMBAL_H
#define __GIMBAL_H

#include <hardware_config.h>
#include <main.h>
#include <motor_config.h>
#include <pid.h>
#include <user_lib.h>
#include "dr16.hpp"

typedef enum {
    GIMBAL_STATE_STOP = 0,  //停止模式
    GIMBAL_STATE_NORMAL,    //普通模式
    GIMBAL_STATE_SPINNING,  //小陀螺模式
} gimbal_state_t;

typedef struct gimbal_behavior {

    volatile int16_t pitch_speed_;   //  枪管电机 速度

    volatile int16_t yaw_degree_;  // Yaw轴电机 角度
    volatile int16_t yaw_speed_;   // Yaw轴电机 速度
} gimbal_behavior_t;

class Gimbal {
   public:
    Gimbal(void);

    int  init(void);
    void set_state(gimbal_state_t state);
    void set_shoot_speed(int16_t shoot_degree);
    void set_yaw_speed(int16_t yaw_speed);
    void update(uint8_t id, uint8_t data[8]);
		void camera_control(void);
		void camera_init(void);
    void set_current(int16_t pitch_motor, int16_t yaw_motor, int16_t plunk_motor_1);
    void pitch_limit(void);
    void motion_solution(void);
    void gimbal_pid_set(void);
    void handler(void);
    void deploy_mode(bool state);
    void manual_mode(int32_t pitch_delta, int16_t yaw_speed);
    void auto_mode(servo_degree_t camera_target, int32_t shoot_target, int16_t yaw_speed);
    void turn_around();

    int16_t        get_shoot(void);
    int16_t        get_yaw(void);

    void debug(void);

    bool yaw_pid_angle_flag_;
    bool shoot_pid_angle_flag_;
   private:
    m3508_t          	pitch_3508_;  // 枪管 6020电机
    gm6020_t          yaw_gm6020_;    // yaw 6020电机
    gimbal_behavior_t behavior_;      // 期望速度角度缓存
    gimbal_state_t    state_;         // 云台状态
    pid_type_t pitch_pid_angle_;   // 枪管电机角度  PID
    pid_type_t pitch_pid_speed_;   // 枪管电机速度 PID
    pid_type_t yaw_pid_speed_;     // yaw轴电机速度 PID

    pid_type_t yaw_pid_angle_;     // yaw轴电机角度 PID

	  ramp_function_source_t ramp_pitch_speed_;          // 摩擦轮斜坡结构体
	 
	 
	 //
	 	fp32    pitch_real_angle;		
		fp32    pitch_expect_angle;
		int16_t pitch_last_angle;
    fp32    pitch_angle;
	 
    int turning_count;

    static void timeout(void *parameter);
    rt_timer_t  timer_;  // 云台电机 PID 计算定时器

};

extern Gimbal gimbal;
extern bool deploy_flag;
#endif
